#include "suniv-clock.h"


/**
 * @brief set CPU clock source
 * @param clk_src CPU clock source, can be one of:
 *                   0 - CLK_CPU_SRC_LOSC
 *                   1 - CLK_CPU_SRC_OSC24M
 *                   2 - CLK_CPU_SRC_PLL_CPU
 * @return none
 */
void clock_cpu_config(int clk_src)
{
    uint32_t reg = read32(CCU_BASE + CCU_CPU_CFG) & ~(0x3 << 16);
    write32(CCU_BASE + CCU_CPU_CFG, reg | (clk_src << 16));
}


/**
 * @brief set HCLK clock divider
 * @param div HCLK clock divider, can be one of:
 *              1 - CLK_HCLK_DIV_1
 *              2 - CLK_HCLK_DIV_2
 *              3 - CLK_HCLK_DIV_4
 *              4 - CLK_HCLK_DIV_8
 * @return none
 */
void clock_hclk_config(uint8_t div)
{
    if((div == 0) || (div > 4)) return;

    uint32_t val = read32(CCU_BASE + CCU_AHB_APB_CFG) & ~(0x3 << 16);
    write32(CCU_BASE + CCU_AHB_APB_CFG, val | ((div - 1) << 16));
}


/**
 * @brief set AHB clock source and divider
 * @param clk_src AHB clock source, can be one of:
 *                  0 - CLK_AHB_SRC_LOSC
 *                  1 - CLK_AHB_SRC_OSC24M
 *                  2 - CLK_AHB_SRC_CPUCLK
 *                  3 - CLK_AHB_SRC_PLL_PERIPH_PREDIV
 * 
 * @param pre_div AHB clock pre-divider, can be 1, 2, 3, 4
 * @param div AHB clock divider, can be 1, 2, 4, 8
 * @return none
 */
void clock_ahb_config(int clk_src, uint8_t pre_div, uint8_t div)
{
    if((pre_div == 0) || (pre_div > 4)) {
        return;
    }

    if((div == 0) || ((div > 4) && (div != 8)) || (div == 3)) {
        return;
    }

    if(div == 4) div = 3;
    if(div == 8) div = 4;

    uint32_t val = read32(CCU_BASE + CCU_AHB_APB_CFG) & ~((0x3 << 12) | (0xF << 4));
    write32(CCU_BASE + CCU_AHB_APB_CFG, val | (clk_src << 12) | ((pre_div - 1) << 6) | ((div - 1) << 4));
}


/**
 * @brief get APB clock frequency
 * @param div: APB clock divider, can be one of:
 *              1 - CLK_APB_DIV_2
 *              2 - CLK_APB_DIV_4
 *              3 - CLK_APB_DIV_8
 *               
 * return none
 */
void clock_apb_config(int div) 
{
    uint32_t val = read32(CCU_BASE + CCU_AHB_APB_CFG) & ~(0x3 << 8);
    write32(CCU_BASE + CCU_AHB_APB_CFG, val | (div << 8));
}


/**
 * @brief initialize CPU PLL
 * @param mul: PLL multiplication factor, can be 1..128
 * @param div: PLL division factor, can be 1..16
 * @return none
 */
void pll_cpu_init(uint8_t mul, uint8_t div)
{
    if((mul == 0) || (div == 0)) {
        return;
    }

    if((mul > 128) || (div > 16)) {
        return;
    }

    uint8_t n, k, m, p;
    // mul = n*k
    // n = 1..32
    // k = 1..4
    for(k = 1; k <= 4; k++) {
        n = mul / k;
        if((n < 32) && (n * k == mul)) break;
    }
    if(n * k != mul) return;
    // div = m*p
    // m = 1..4
    // k = 1,2,4
    for(m = 1; m <= 4; m++) {
        p = div / m;
        if(((p == 1) || (p == 2) || (p == 4)) && (m * p == div)) break;
    }

    if(m * p != div) return;

    p--;
    if(p == 3) p = 2;

    uint32_t val = read32(CCU_BASE + CCU_PLL_CPU_CTRL);
    val &= (1 << 31) | (1 << 28);
    val |= ((n - 1) << 8) | ((k - 1) << 4) | (m - 1) | (p << 16);
    write32(CCU_BASE + CCU_PLL_CPU_CTRL, val);
}


/**
 * @brief initialize peripheral PLL
 * @param mul: PLL multiplication factor, can be 1..128
 * @param div: PLL division factor, can be 1..16
 * @return none
 */
void pll_periph_init(uint8_t mul, uint8_t div)
{
    if((mul == 0) || (div == 0)) return;
    if((mul > 32) || (div > 4)) return;

    // mul = n
    // div = m

    uint32_t val = read32(CCU_BASE + CCU_PLL_PERIPH_CTRL);
    val &= (1 << 31) | (1 << 28);
    val |= ((mul - 1) << 8) | ((div - 1) << 4) | (1 << 18);
    write32(CCU_BASE + CCU_PLL_PERIPH_CTRL, val);
}

